/*
 * 单个电机的指令控制代码
 */
#include <MultiStepper.h>
#include<AccelStepper.h>
AccelStepper tFstepper(1, 5, 4);

char cmd;        //电机指令字符
long data;        //电机指令参数
boolean i = 1;
int isread = 0;
int mode = 1;
float stopTime = 0;
long sub = 200;
float t = 0;
float ti = millis();
void motorMode(boolean mode);
void finitialattach();
void setup()
{
    Serial.begin(9600);
    Serial.println(F("++++++++++++++++++++++++++++++++++"));
    Serial.println(F("+ Taichi-Maker AccelStepper Demo +"));
    Serial.println(F("+     www.taichi-maker.com       +"));
    Serial.println(F("++++++++++++++++++++++++++++++++++"));
    Serial.println(F(""));
    Serial.println(F("Please input motor command:"));
    Serial.print(F("请输入运行模式:"));
    Serial.println(F("全加速 1 ，匀速 2"));
    tFstepper.setEnablePin(7); //启动使能端口，默认使能关闭。正常工作需调用d1指令打开
    while (!Serial.available()){}
    mode = Serial.parseInt();
    Serial.print(F("请输入电机细分:"));
    while (!Serial.available()){}
    sub = Serial.parseInt();
    Serial.println(sub);
    Serial.print("mode:");
    Serial.println(mode);
//    attachInterrupt(1, finitialattach, FALLING);
//    pinMode(2,INPUT);
}


void loop() {
    if (Serial.available()) {     // 检查串口缓存是否有数据等待传输 
        cmd = Serial.read();        // 获取电机指令中指令信息    
        Serial.print(F("cmd = "));
        Serial.print(cmd);
        Serial.print(F(" , "));

        data = Serial.parseInt();   // 获取电机指令中参数信息  
        Serial.print(F("data = "));
        Serial.print(data);
        Serial.println(F(""));

        runUsrCmd();
    }
    motorMode(mode);
    if (tFstepper.distanceToGo() == 0 && isread && mode != 2)
    {
        Serial.print(F("current position:"));
        Serial.println((float)(tFstepper.currentPosition()*360/sub));
        Serial.print(F("current speed:"));
        Serial.println((float)(tFstepper.speed() * 360 / sub));
        Serial.print(F("total time:"));
        Serial.println((float)((millis() - t) / 1000));
        isread = !isread;
    }

}


void runUsrCmd() {
    switch (cmd) {
    case 'o': //利用currentPosition获取当前电机输出轴位置并通过串口监视器输出该信息
        Serial.print(F("Motor Position: "));
        Serial.println((float)(tFstepper.currentPosition()*360/sub));
        break;

    case 'v': //利用moveTo函数使电机运行到用户指定坐标位置，moveTo函数不会“block”程序。

        Serial.print(F("Motor 'moveTo' "));  Serial.println((long)(data));
        tFstepper.moveTo((long)(data * sub / 360));
        t = millis();
        isread = !isread;
        break;

    case 'm':   // 利用move函数使电机运行相应步数。move函数不会“block”程序。

        Serial.print(F("Motor 'move'  "));  Serial.println((long)(data));
        tFstepper.move((long)(data * sub / 360));
        t = millis();
        isread = !isread;
        break;

    case 'r':  //利用runToNewPosition函数让电机运行到用户指定位置值。  
        Serial.print(F("Motor 'runToNewPosition' "));
        Serial.println((long)(data));
        tFstepper.runToNewPosition((long)(data * sub / 360));
        t = millis();
        isread = !isread;
        break;

    case 's':   //利用setCurrentPosition函数设置当前位置为用户指定位置值。
        Serial.print(F("Set Motor Current Position to "));
        Serial.println((long)(data * sub / 360));
        tFstepper.setCurrentPosition((long)(data * sub / 360));
        break;

    case 'a':  //利用setAcceleration函数设置加速度 
        Serial.print(F("Motor 'setAcceleration' "));
        Serial.println((float)(data));
        tFstepper.setAcceleration((float)(data * sub / 360));
        break;

    case 'x':  //利用setMaxSpeed函数设置最大速度   
        Serial.print(F("Motor 'setMaxSpeed' "));
        Serial.println((float)(data));
        tFstepper.setMaxSpeed((float)(data * sub / 360));
        break;

    case 'd':  //用户通过此指令可控制电机使能的开关，如果驱动器使能未连接，则该指令无效  
        if (data == 1)
            tFstepper.disableOutputs();
        else
            tFstepper.enableOutputs();
        break;
    case 'q'://用以指定加速度匀加速度使电机停止运动
        Serial.print(F("when stop,current speed:"));
        Serial.println((float)(tFstepper.speed()*360/sub));
        tFstepper.setAcceleration((float)(data * sub / 360));
        tFstepper.stop();
        stopTime = millis();
        tFstepper.runToPosition();
        Serial.print(F("停止所花时间:"));
        Serial.println((millis() - stopTime) / 1000);
        break;
    case 'p'://用以强制停止
        tFstepper.setCurrentPosition(tFstepper.currentPosition());
        Serial.println("已经强制停止");
        Serial.print("current speed:");
        Serial.println((float)(tFstepper.speed()*360/sub));
        break;
    case 'f'://用以显示当前速度
        Serial.print("current speed:");
        Serial.println((float)(tFstepper.speed() * 360 / sub));
        break;
    case 'b':用以修改当前速度
        if(data > tFstepper.maxSpeed()){
          Serial.println("超出最大速度，自动修改最大速度");
          tFstepper.setMaxSpeed((float)(data * sub / 360));
        }
        tFstepper.setSpeed((float)(data * sub / 360));
        Serial.print("set speed:");
        Serial.println((float)(tFstepper.speed() * 360 / sub));
    default:  // 未知指令
        Serial.println(F("Unknown Command"));
    }
}
void motorMode(int mode)
{
    if (mode == 1)
    {
        if (i)
        {
            Serial.println("请设置初始加速度和最大速度");
            i = 0;

        }
        if (tFstepper.currentPosition() % (sub *5) == 0 && tFstepper.isRunning())
        {
            Serial.print(F("current position:"));
            Serial.println((float)(tFstepper.currentPosition()*360/sub));
            Serial.print(F("current speed:"));
            Serial.println((float)(tFstepper.speed() * 360 / sub));

        }
        tFstepper.run();
    }
    //else if (mode == 0)
    //{

    //}

    else if(mode == 2)
    {
        if (i) {
            int initialspeed,initialMaxspeed;

            ti = millis();
            i = 0;
            tFstepper.setCurrentPosition(0);

            
            Serial.println(F("please set a initial Maxspeed"));
            while (!Serial.available()){}
            initialMaxspeed = Serial.parseInt();
            Serial.print(F("the initial Maxspeed is "));
            Serial.println((float)(initialMaxspeed));
            tFstepper.setMaxSpeed((float)(initialMaxspeed * sub / 360));

            
            Serial.println(F("please set a initial speed"));
            while (!Serial.available()){}
            initialspeed = Serial.parseInt();
            Serial.print(F("the initial speed is "));
            Serial.println((float)(initialspeed));
            tFstepper.setSpeed((float)(initialspeed * sub / 360));

        }
            if (tFstepper.currentPosition() % (sub *5) == 0 && tFstepper.isRunning())
            {
                Serial.print(F("current position:"));
                Serial.println((float)(tFstepper.currentPosition()*360/sub));
                Serial.print(F("current speed:"));
                Serial.println((float)tFstepper.speed()*360/sub);
              Serial.print("current time:");
              Serial.println((float)((millis()-ti)/1000));
            }
           tFstepper.runSpeed();
    }
}
void finitialattach()
{
  tFstepper.setCurrentPosition(0);
  Serial.println("触发中断");
//  delay(100);
}
